#include "Geom_HelixCurve.h"

#include <gp_Vec.hxx>
#include <Geom_Line.hxx>
#include <Precision.hxx>

IMPLEMENT_STANDARD_RTTIEXT(Geom_HelixCurve, Geom_Curve)

Geom_HelixCurve::Geom_HelixCurve(const gp_Ax3& lcs, double radius, double pitch)
    : _r(radius), _pitch(pitch), _reversed(false), _lcs(lcs)
{
    _trsf.SetTransformation(_lcs, gp::XOY());
}

void Geom_HelixCurve::Reverse()
{
    _reversed = !_reversed;
}

Standard_Real Geom_HelixCurve::ReversedParameter(const Standard_Real U) const
{
    return -U;
}

Standard_Real Geom_HelixCurve::FirstParameter() const
{
    return -Precision::Infinite();
}

Standard_Real Geom_HelixCurve::LastParameter() const
{
    return Precision::Infinite();
}

Standard_Boolean Geom_HelixCurve::IsClosed() const
{
    return false;
}

Standard_Boolean Geom_HelixCurve::IsPeriodic() const
{
    return false;
}

GeomAbs_Shape Geom_HelixCurve::Continuity() const
{
    return GeomAbs_CN;
}

Standard_Boolean Geom_HelixCurve::IsCN(const Standard_Integer N) const
{
    return true;
}

void Geom_HelixCurve::D0(const Standard_Real U, gp_Pnt& P) const
{
    P.SetCoord(_r * std::cos(U), _r * std::sin(U), U * _pitch / (2 * M_PI));
    P.Transform(_trsf);
}

void Geom_HelixCurve::D1(const Standard_Real U, gp_Pnt& P, gp_Vec& V1) const
{
    D0(U, P);
    V1.SetCoord(-_r * std::sin(U), _r * std::cos(U), _pitch / (2 * M_PI));
    V1.Transform(_trsf);
}

void Geom_HelixCurve::D2(const Standard_Real U, gp_Pnt& P, gp_Vec& V1, gp_Vec& V2) const
{
    D1(U, P, V1);
    V2.SetCoord(-_r * std::cos(U), -_r * std::sin(U), 0);
    V2.Transform(_trsf);
}

void Geom_HelixCurve::D3(const Standard_Real U, gp_Pnt& P, gp_Vec& V1, gp_Vec& V2, gp_Vec& V3) const
{
    D2(U, P, V1, V2);
    V3.SetCoord(_r * std::sin(U), -_r * std::cos(U), 0);
    V3.Transform(_trsf);
}

gp_Vec Geom_HelixCurve::DN(const Standard_Real U, const Standard_Integer N) const
{
    Standard_RangeError_Raise_if(N <= 0, "Error At DN of Geom_HelixCurve");

    gp_Vec d;
    gp_Pnt ptmp;
    gp_Vec vtmp1, vtmp2;
    switch (N % 4)
    {
    case 1: D1(U, ptmp, d); break;
    case 2: D2(U, ptmp, vtmp1, d); break;
    case 3: D3(U, ptmp, vtmp1, vtmp2, d); break;
    default:
    {
        gp_Pnt p;
        D0(U, p);
        d.SetXYZ(p.XYZ());
    } break;
    }
    if (N >= 2)
    {
        d.SetZ(0);
    }

    return gp_Vec();
}

void Geom_HelixCurve::Transform(const gp_Trsf& T)
{
    _trsf.PreMultiply(T);
    _lcs.Transform(T);
}

Handle(Geom_Geometry) Geom_HelixCurve::Copy() const
{
    Handle(Geom_HelixCurve) copy = new Geom_HelixCurve(_lcs, _r, _pitch);
    if (_reversed)
    {
        copy->Reverse();
    }
    return copy;
}
